import random
import cv2
import numpy as np
from cv_bridge import CvBridge

import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Image

class PublisherSimCamera(Node):

    def __init__(self):
        super().__init__('sim_camera_rgb')
        self.pub_rgb = self.create_publisher(Image, '/sim/camera/rgb', 1)
        timer_period = 1  # seconds
        self.timer = self.create_timer(timer_period, self.timer_callback)

        # opencv init
        self.bridge = CvBridge()
        self.init_img()
    
    def init_img(self):
        self.width = 100
        self.height = 100
        self.img = np.zeros((self.height,self.width,3), np.uint8)
        self.count = 0

    def timer_callback(self):
        img = self.img
        blue = random.randint(0, 255)
        green = random.randint(0, 255)
        red = random.randint(0, 255)
        img[:, :] = (blue, green, red)
        cv2.imshow("rgb", img)
        cv2.waitKey(10)

        rgb_img = self.bridge.cv2_to_imgmsg(img, encoding="rgb8")
        rgb_img.header.stamp = self.get_clock().now().to_msg()
        rgb_img.header.frame_id = "sim_camera_frame"  # 设置坐标系
        
        self.pub_rgb.publish(rgb_img)
        self.count += 1


def main(args=None):
    rclpy.init(args=args)
    app_node = PublisherSimCamera()
    rclpy.spin(app_node)
    app_node.destroy_node()
    rclpy.shutdown()


if __name__ == '__main__':
    main()